#include "ServoDrive.h"

ECatDrv::ServoDrive::ServoDrive(unsigned int eep_man, unsigned int eep_id, const std::string &slave_name,
                                int slave_addr, ECatDrv::slaveType slave_type)
    :EthercatSlave(eep_man, eep_id, slave_name, slave_addr, slave_type)
{
    m_state.stateVal = 0x0;
    m_state.opVal = 0x0;

    m_control.ServoEnable = false;
    m_control.op = ServoOpMode::OP_CSV;     // 默认为速度模式
    m_control.positionCmd = 0;
    m_control.velocityCmd = 0;
}

void ECatDrv::ServoDrive::readSDO(uint16_t addr, int *size, uint8_t *buf)
{
    int __ret = ec_SDOread(slave_addr, addr, 0x0, FALSE, size, buf, EC_TIMEOUTRXM);
    //    qDebug() << "Slave: " << slave << " - Read from 0x" << hex << addr << dec << ", size: " << size << ", res = " << __ret;
    (void) __ret;
}

/**
 * @brief 伺服状态更新
 */
void ECatDrv::ServoDrive::StMachineUpdate()
{
    GetSlaveSdo(CIA_STATE_WORD, 2, (unsigned int*)&m_state.stateVal);
    GetSlaveSdo(CIA_ERROR_CODE, 2, (unsigned int*)&m_state.errorCode);
    GetSlaveSdo(CIA_OP_MODE_DISPLAY, 1, (unsigned int*)&m_state.opVal);

}



std::string ECatDrv::ServoDrive::GetVersionStr()
{
    GetVersion();
    std::string ver;
    ver = std::to_string(m_state.version[3]) + std::string(".") +
          std::to_string(m_state.version[2]) + std::string(".") +
          std::to_string(m_state.version[1]) + std::string(".") + std::to_string(m_state.version[0]) ;
    return ver;
}

void ECatDrv::ServoDrive::GetVersion()
{
    int size = 4;
    readSDO(CIA_APP_VERSION, &size, m_state.version);
//    qDebug() << "ver: " << m_state.version[3] << "." << m_state.version[2] << "." << m_state.version[1] << "." << m_state.version[0];
}

bool ECatDrv::ServoDrive::isBootloaderState(void)
{
    if(m_state.version[0])
    {
        return true;
    }
    else
    {
        return false;
    }
}

void ECatDrv::ServoDrive::GetSlaveState(uint16_t &state, uint8_t &op)
{
    state = m_state.stateVal;
    op = m_state.opVal;
}

void ECatDrv::ServoDrive::GetFaultInfo(uint16_t &faultcode, std::string &error)
{
    faultcode = m_state.errorCode;
    error = "undefined error description";
}


int ECatDrv::ServoDrive::GetSlaveSdo(int addr, int size, unsigned int* recv)
{
    int len;

    if(size == 4)
    {
        len = 4;
        ec_SDOread(slave_addr, addr, 0x0, FALSE, &len, recv, EC_TIMEOUTRXM);
    }
    else if(size == 2)
    {
        len = 2;
        ec_SDOread(slave_addr, addr, 0x0, FALSE, &len, recv, EC_TIMEOUTRXM);
    }
    else if(size == 1)
    {
        len = 1;
        ec_SDOread(slave_addr, addr, 0x0, FALSE, &len, recv, EC_TIMEOUTRXM);
    }
    else
    {
        len = size;
        ec_SDOread(slave_addr, addr, 0x0, FALSE, &len, recv, EC_TIMEOUTRXM);
    }

    return 0;
}

/**
 * @brief 设置从站sdo数据
 * @param slave 1~n
 * @param addr
 * @param size
 * @param data
 * @return
 */
int ECatDrv::ServoDrive::SetSlaveSdo(uint16 addr, int size, unsigned int* data)
{
    uint32 fb_4;
    uint16 fb_2;
    uint8 fb_1;
    int wkc;
    if (size == 4)
    {
        fb_4 = *data;
        wkc = ec_SDOwrite(slave_addr, addr, 0x0, FALSE, size, &fb_4, EC_TIMEOUTTXM);
       qDebug() << "fb_4: size=" << size << ", wkc=" << wkc << ", addr=" << addr << ", val="<< fb_4;
    }
    else if (size==2)
    {
        fb_2 = *data;
        wkc = ec_SDOwrite(slave_addr, addr, 0x0, FALSE, size, &fb_2, EC_TIMEOUTTXM);
        qDebug() << "fb_2: size=" << size << ", wkc=" << wkc << ", addr=" << addr << ", val=" << fb_2;
    }
    else if (size == 1)
    {
        fb_1 = *data;
        wkc = ec_SDOwrite(slave_addr, addr, 0x0, FALSE, size, &fb_1, EC_TIMEOUTTXM);
       qDebug() << "fb_1: size=" << size << ", wkc=" << wkc << ", addr=" << addr << ", val=" << fb_1;
    }
    else if (size == 64)
    {
        wkc = ec_SDOwrite(slave_addr, addr, 0x0, FALSE, size, data, EC_TIMEOUTTXM);
    }
    else
    {
        wkc = 0;
    }
    return wkc;
}

bool ECatDrv::ServoDrive::ServoEnable()
{
    if(m_state.errorCode != 0)
    {
        return false;
    }
    m_control.ServoEnable = true;

    return true;
}

void ECatDrv::ServoDrive::ServoDisable(void)
{
    m_control.ServoEnable = false;
}


void ECatDrv::ServoDrive::ClearFault()
{
    if(m_state.errorCode != 0 || m_state.ctrlMode == ServoControlMode::Fault)
    {
        unsigned int val = 0x80;
        SetSlaveSdo(CIA_CONTROL_WORD, 2, &val);  // clear fault
        qDebug() << "清错执行";
    }
}

void ECatDrv::ServoDrive::JogOn(int vel)
{

}

void ECatDrv::ServoDrive::JogOff()
{

}

void ECatDrv::ServoDrive::initPdoOutput()
{

}

void ECatDrv::ServoDrive::initPdoInput()
{

}
